#include "tb6600.h"
#include <stdio.h>

// 使用教程：
// 1. 使用 TB6600_Init_TIM 或 TB6600_Init_IO 配置TB6600参数
//----------------------------------------------------------------

/*----------------------------变量定义----------------------------*/
TB6600_TaskTypedef TB6600_Task = TB6600_DEFAULT_TASK;


/*----------------------------函数实现----------------------------*/
/**
 *@brief TB6600 使能状态更新
 *@param state TB6600的状态结构体
 *@note 需要外部实现state->State_Update参数逻辑，本函数不做任何处理
 */
 void TB6600_Update_Ena(TB6600_StateTypedef *state){
    // 更新TB6600使能
    if(state->Config->Circuit_AC == TB6600_Circuit_Anode)
        HAL_GPIO_WritePin(state->Config->ENA_GPIOx, state->Config->ENA_GPIO_Pin, \
            state->Ena == TB6600_Enable_Enable? GPIO_PIN_RESET : GPIO_PIN_SET);
    else
        HAL_GPIO_WritePin(state->Config->ENA_GPIOx, state->Config->ENA_GPIO_Pin, \
            state->Ena == TB6600_Enable_Enable? GPIO_PIN_SET : GPIO_PIN_RESET);
}

/**
 *@brief TB6600 方向状态更新
 *@param state TB6600的状态结构体
 *@note 需要外部实现state->State_Update参数逻辑，本函数不做任何处理
 */
void TB6600_Update_Dir(TB6600_StateTypedef *state){
    // 更新TB6600方向
    if(state->Config->Circuit_AC == TB6600_Circuit_Anode)
        HAL_GPIO_WritePin(state->Config->DIR_GPIOx, state->Config->DIR_GPIO_Pin, \
            state->Dir == TB6600_Direction_CW? GPIO_PIN_RESET : GPIO_PIN_SET);
    else {
        HAL_GPIO_WritePin(state->Config->DIR_GPIOx, state->Config->DIR_GPIO_Pin, \
            state->Dir == TB6600_Direction_CW? GPIO_PIN_SET : GPIO_PIN_RESET);
    }
}

/**
 *@brief TB6600 速度状态更新
 *@param state TB6600的状态结构体
 *@note 需要外部实现state->State_Update参数逻辑，但对TB6600_Task中的标志位进行了处理。
 */
 void TB6600_Update_Speed(TB6600_StateTypedef *state){
    // 更新TB6600速度
    __HAL_TIM_SET_PRESCALER(state->Config->htim, (uint16_t)((1000000.0 / (double)state->Speed_Target) - 1));
    TB6600_Task.Tim1_SpeedUpdate = Speed_Change_None;
}

/**
 *@brief 初始化TB6600(TIM脉冲模式)
 *@param init_config 初始化参数
 *@return TB6600_ConfigTypedef TB6600的初始化结构体
 */
TB6600_ConfigTypedef TB6600_Init_TIM(TB6600_ConfigTypedef init_config) {
    TB6600_ConfigTypedef tb6600_config = TB6600_DEFAULT_CONFIG;
    
    tb6600_config.MicroStep     = init_config.MicroStep;
    tb6600_config.Circuit_AC    = init_config.Circuit_AC;
    tb6600_config.PulMode       = TB6600_PUL_Mode_TIM;
    tb6600_config.htim          = init_config.htim;
    tb6600_config.TIM_Channel   = init_config.TIM_Channel;
    tb6600_config.DIR_GPIOx     = init_config.DIR_GPIOx;
    tb6600_config.DIR_GPIO_Pin  = init_config.DIR_GPIO_Pin;
    tb6600_config.ENA_GPIOx     = init_config.ENA_GPIOx;
    tb6600_config.ENA_GPIO_Pin  = init_config.ENA_GPIO_Pin;

    if(init_config.Step_Angle) tb6600_config.Step_Angle = init_config.Step_Angle;
    if(init_config.Speed_Max_rpm) tb6600_config.Speed_Max_rpm = init_config.Speed_Max_rpm;
    if(init_config.Circle_Max) tb6600_config.Circle_Max = init_config.Circle_Max;

    // 对应细分 -> 步数每圈
    switch (tb6600_config.MicroStep) {
        case TB6600_Microstep_NC: tb6600_config.Pulse_Rev   = NULL; break;
        case TB6600_Microstep_1: tb6600_config.Pulse_Rev    = 200;  break;
        case TB6600_Microstep_2_A: tb6600_config.Pulse_Rev  = 400;  break;
        case TB6600_Microstep_2_B: tb6600_config.Pulse_Rev  = 400;  break;
        case TB6600_Microstep_4: tb6600_config.Pulse_Rev    = 800;  break;
        case TB6600_Microstep_8: tb6600_config.Pulse_Rev    = 1600; break;
        case TB6600_Microstep_16: tb6600_config.Pulse_Rev   = 3200; break;
        case TB6600_Microstep_32: tb6600_config.Pulse_Rev   = 6400; break;
        default: break;
    }
   
     tb6600_config.Speed_Max_hz = tb6600_config.Speed_Max_rpm/60.0*tb6600_config.Pulse_Rev;
     tb6600_config.Step_Max     = tb6600_config.Circle_Max*tb6600_config.Pulse_Rev;

    return tb6600_config;
}

/**
 *@brief 初始化TB6600(GPIO脉冲模式)
 *@param init_config 初始化参数
 *@return TB6600_ConfigTypedef TB6600的初始化结构体
 */
TB6600_ConfigTypedef TB6600_Init_IO(TB6600_ConfigTypedef init_config) {
    TB6600_ConfigTypedef tb6600_config = TB6600_DEFAULT_CONFIG;
    
    tb6600_config.MicroStep     = init_config.MicroStep;
    tb6600_config.Circuit_AC    = init_config.Circuit_AC;
    tb6600_config.PulMode       = TB6600_PUL_Mode_GPIO;
    tb6600_config.PUL_GPIOx     = init_config.PUL_GPIOx;
    tb6600_config.PUL_GPIO_Pin  = init_config.PUL_GPIO_Pin;
    tb6600_config.DIR_GPIOx     = init_config.DIR_GPIOx;
    tb6600_config.DIR_GPIO_Pin  = init_config.DIR_GPIO_Pin;
    tb6600_config.ENA_GPIOx     = init_config.ENA_GPIOx;
    tb6600_config.ENA_GPIO_Pin  = init_config.ENA_GPIO_Pin;

    if(init_config.Step_Angle) tb6600_config.Step_Angle = init_config.Step_Angle;
    if(init_config.Speed_Max_rpm) tb6600_config.Speed_Max_rpm = init_config.Speed_Max_rpm;
    if(init_config.Circle_Max) tb6600_config.Circle_Max = init_config.Circle_Max;

    // 对应细分 -> 步数每圈
    switch (tb6600_config.MicroStep) {
        case TB6600_Microstep_NC: tb6600_config.Pulse_Rev   = NULL; break;
        case TB6600_Microstep_1: tb6600_config.Pulse_Rev    = 200;  break;
        case TB6600_Microstep_2_A: tb6600_config.Pulse_Rev  = 400;  break;
        case TB6600_Microstep_2_B: tb6600_config.Pulse_Rev  = 400;  break;
        case TB6600_Microstep_4: tb6600_config.Pulse_Rev    = 800;  break;
        case TB6600_Microstep_8: tb6600_config.Pulse_Rev    = 1600; break;
        case TB6600_Microstep_16: tb6600_config.Pulse_Rev   = 3200; break;
        case TB6600_Microstep_32: tb6600_config.Pulse_Rev   = 6400; break;
        default: break;
    }
    tb6600_config.Speed_Max_hz = tb6600_config.Speed_Max_rpm/60.0*tb6600_config.Pulse_Rev;
    tb6600_config.Step_Max     = tb6600_config.Circle_Max*tb6600_config.Pulse_Rev;
    
    return tb6600_config;
}

/**
 *@brief TB6600 运行状态初始化
 *@param init_state 初始化参数
 *@return TB6600_StateTypedef TB6600的状态结构体
 */
TB6600_StateTypedef TB6600_State_Init(TB6600_StateTypedef init_state) {
    TB6600_StateTypedef State = TB6600_DEFAULT_STATE;

    State.Config        = init_state.Config;
    State.Speed_Target  = init_state.Speed_Target;

    if (init_state.Ena)         State.Ena           = init_state.Ena;
    if (init_state.Auto_Speed)  State.Auto_Speed    = init_state.Auto_Speed;
    if (init_state.Acceleration)State.Acceleration  = init_state.Acceleration;

    TB6600_Update_Ena(&State);
    TB6600_Update_Speed(&State);
    return State;
}

/**
 *@brief TB6600 速度设置
 *@param tb6600_state TB6600的状态结构体
 *@param speed 转速 (Hz)(每秒转动次数)
 */
 void TB6600_SetSpeed_hz(TB6600_StateTypedef *tb6600_state, uint32_t speed){
    // 数据检查
    if(speed < 20 || speed > 1000000 || speed > tb6600_state->Config->Speed_Max_hz) return;
    // 幅值
    tb6600_state->Speed_Target = speed;
    // 更改变化标志位
    #ifdef __TIM1_ENABLE__
    if(tb6600_state->Config->htim == &htim1)
        TB6600_Task.Tim1_SpeedUpdate = Speed_Change_True;
    #endif
    #ifdef __TIM2_ENABLE__
    if(tb6600_state->Config->htim == &htim2)
        TB6600_Task.Tim2_SpeedUpdate = Speed_Change_True;
    #endif
    #ifdef __TIM3_ENABLE__
    if(tb6600_state->Config->htim == &htim3)
        TB6600_Task.Tim3_SpeedUpdate = Speed_Change_True;
    #endif
    #ifdef __TIM4_ENABLE__
    if(tb6600_state->Config->htim == &htim4)
        TB6600_Task.Tim4_SpeedUpdate = Speed_Change_True;
    #endif
    tb6600_state->State_Update = TB6600_STATE_Update_True;
}

/**
 *@brief TB6600 速度设置
 *@param tb6600_state TB6600的状态结构体
 *@param speed 转速 (rpm)(每分钟圈数)
 */
void TB6600_SetSpeed_rpm(TB6600_StateTypedef *tb6600_state, uint32_t speed) {
    // 转速 -> 频率
    speed = tb6600_state->Config->Pulse_Rev * speed / 60.0;
    TB6600_SetSpeed_hz(tb6600_state, speed);
}

/**
 *@brief TB6600 自动转速模式设置
 *@param tb6600_state TB6600的状态结构体
 *@param auto_speed 自动转速模式 TB6600_AutoSpeed_Enable(开启) / TB6600_AutoSpeed_Disable(关闭)
 *@note 开启自动转速后，系统会在设置的速度的基础上动态调整运动过程中的速度，使之更加丝滑。
 */
void TB6600_Set_AutoSpeed(TB6600_StateTypedef *tb6600_state, TB6600_AutoSpeed auto_speed){
    tb6600_state->Auto_Speed = auto_speed;
    tb6600_state->State_Update = TB6600_STATE_Update_True;
}

/**
 *@brief TB6600 使能/失能
 *@param tb6600_state TB6600的状态结构体
 *@param ena 使能/失能 TB6600_Enable_Enable(使能) / TB6600_Enable_Disable(失能)
 */
void TB6600_Set_Ena(TB6600_StateTypedef *tb6600_state, TB6600_Enable ena){
    tb6600_state->Ena = ena;
    tb6600_state->State_Update = TB6600_STATE_Update_True;
}

/**
 *@brief TB6600 转动给定步数
 *@param tb6600_state TB6600的状态结构体
 *@param step_num 需要转动步数
 *@param dir 方向 TB6600_Direction_CW(顺时针) / TB6600_Direction_CCW(逆时针)
 */
void TB6600_Move_Step(TB6600_StateTypedef *tb6600_state, uint32_t step_num, TB6600_Direction dir){
    // 数据检查
    if(((tb6600_state->Step_Target + step_num > tb6600_state->Config->Step_Max)\
        && dir == TB6600_Direction_CW) ||\
        ((tb6600_state->Step_Target < step_num) && dir == TB6600_Direction_CCW)) {
            return;
    }

    // 更新剩余步数
    #ifdef __TIM1_ENABLE__
        if(tb6600_state->Config->htim == &htim1){
            if(dir == TB6600_Direction_CW)
                TB6600_Task.Tim1_Step += step_num;
            else
                TB6600_Task.Tim1_Step -= step_num;
        }
    #endif
    #ifdef __TIM2_ENABLE__
        if(tb6600_state->Config->htim == &htim2){
            if(dir == TB6600_Direction_CW)
                TB6600_Task.Tim2_Step += step_num;
            else
                TB6600_Task.Tim2_Step -= step_num;
        }
    #endif
    #ifdef __TIM3_ENABLE__
        if(tb6600_state->Config->htim == &htim3){
            if(dir == TB6600_Direction_CW)
                TB6600_Task.Tim3_Step += step_num;
            else
                TB6600_Task.Tim3_Step -= step_num;
        }
    #endif
    #ifdef __TIM4_ENABLE__
        if(tb6600_state->Config->htim == &htim4){
            if(dir == TB6600_Direction_CW)
                TB6600_Task.Tim4_Step += step_num;
            else
                TB6600_Task.Tim4_Step -= step_num;
        }
    #endif

    // 设置目标步数
    if (dir == TB6600_Direction_CW){
        tb6600_state->Step_Target += step_num;
    }
    else{
        tb6600_state->Step_Target -= step_num;
    }
    tb6600_state->State_Update = TB6600_STATE_Update_True;

    TB6600_Move_Prog(tb6600_state);
}

/**
 *@brief TB6600 转动给定角度
 *@param tb6600_state TB6600的状态结构体
 *@param angle 需要转动角度 (°)
 */
void TB6600_Move_angle(TB6600_StateTypedef *tb6600_state, double angle){
    // 转动角度 -> 转动步数
    double step_change = (double)tb6600_state->Config->Pulse_Rev / 360.0 * angle;

    if (angle <= 0){
        TB6600_Move_Step(tb6600_state, (uint32_t)step_change, TB6600_Direction_CCW);
    }
    else{
        TB6600_Move_Step(tb6600_state, (uint32_t)step_change, TB6600_Direction_CW);
    }
}

/**
 *@brief TB6600 主程序
 *@param tb6600_state TB6600的状态结构体
 *@note 应将此函数放入主循环中
 */
void TB6600_Move_Prog(TB6600_StateTypedef *state){
    // TODO
    if(state->State_Update == TB6600_STATE_Update_True){
        TB6600_Update_Ena(state);
        TB6600_Update_Speed(state);
        state->State_Update = TB6600_STATE_Update_None;
    }
    
    // 更新当前转过步数
    #ifdef __TIM1_ENABLE__
    if(state->Config->htim == &htim1){
        state->Step_Now = state->Step_Target - TB6600_Task.Tim1_Step;
    }
    #endif
    #ifdef __TIM2_ENABLE__
    if(state->Config->htim == &htim2){
        state->Step_Now = state->Step_Target - TB6600_Task.Tim2_Step;
    }
    #endif
    #ifdef __TIM3_ENABLE__
    if(state->Config->htim == &htim3){
        state->Step_Now = state->Step_Target - TB6600_Task.Tim3_Step;
    }
    #endif
    #ifdef __TIM4_ENABLE__
    if(state->Config->htim == &htim4){
        state->Step_Now = state->Step_Target - TB6600_Task.Tim4_Step;
    }
    #endif
    
    // 计算剩余步数并设置旋转方向
    int32_t step = state->Step_Target - state->Step_Now;
    if(step > 0){
        state->Dir = TB6600_Direction_CW;
    }
    else if(step < 0){
        state->Dir = TB6600_Direction_CCW;
        step = -step;
    }
    TB6600_Update_Dir(state);

    // 更新剩余步数
    # ifdef __TIM1_ENABLE__
    if(state->Config->htim == &htim1){
        TB6600_Task.Tim1_Step = step;
    }
    #endif
    #ifdef __TIM2_ENABLE__
    else if(state->Config->htim == &htim2){
        TB6600_Task.Tim2_Step = step;
    }
    #endif
    #ifdef __TIM3_ENABLE__
    else if(state->Config->htim == &htim3){
        TB6600_Task.Tim3_Step = step;
    }
    #endif
    #ifdef __TIM4_ENABLE__
    else if(state->Config->htim == &htim4){
        TB6600_Task.Tim4_Step = step;
    }
    #endif

    // 防止之前电机停转
    if(step != 0){
        HAL_TIM_PWM_Start(state->Config->htim, state->Config->TIM_Channel);
        HAL_Delay(1);
    }
    
}

void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim){
    #ifdef __TIM1_ENABLE__
    if(htim->Instance == TIM1){
        TB6600_Task.Tim1_Step--;
        // 启停PWM，并更新剩余步数
        if(TB6600_Task.Tim1_Step == 0)
            HAL_TIM_PWM_Stop(htim, htim->Channel);
    }
    #endif
    #ifdef __TIM2_ENABLE__
    if(htim->Instance == TIM2){
        TB6600_Task.Tim2_Step--;
        // 启停PWM，并更新剩余步数
        if(TB6600_Task.Tim2_Step == 0)
            HAL_TIM_PWM_Stop(htim, htim->Channel);
    }
    #endif
    #ifdef __TIM3_ENABLE__
    if(htim->Instance == TIM3){
        TB6600_Task.Tim3_Step--;
        // 启停PWM，并更新剩余步数
        if(TB6600_Task.Tim3_Step == 0)
            HAL_TIM_PWM_Stop(htim, htim->Channel);
    }
    #endif
    #ifdef __TIM4_ENABLE__
    if(htim->Instance == TIM4){
        TB6600_Task.Tim4_Step--;
        // 启停PWM，并更新剩余步数
        if(TB6600_Task.Tim4_Step == 0)
            HAL_TIM_PWM_Stop(htim, htim->Channel);
    }
    #endif
}

